/*
 * servo.c
 *
 * Created: 11/5/2013 7:13:37 PM
 *  Author: Tim
 */ 

#include "../src/Epiphany.h"

void pwmSetup();

void setPulseLength(uint16_t length);

void setup()
{
	//put all your initializations here
	clockInit();
	pwmSetup();
	RTC_DelayInit();
}

void loop()
{

	//configure the for loop to move the servo from 0 degrees to 180 degrees	
	for(;;)
	{
		
		//control the pulse with the commented out line below
		
//		setPulseLength();

		//this is a delay used to prevent updating the servo faster than it can respond
		while(delayNotOver)
		{
			RTC_Delay_ms(20);
		}
	}

	//configure the for loop to move the servo from 180 degrees to 0 degrees	
	for(;;)
	{
		
		//control the pulse with the commented out line below
		
//		setPulseLength();

		//this is a delay used to prevent updating the servo faster than it can respond
		while(delayNotOver)
		{
			RTC_Delay_ms(20);
		}
	}	
}

void setPulseLength(uint16_t length)
{
	//make sure we don't break the servo by sending a erroneous pulse
	if(length < 300) length = 300;
	else if(length > 1200) length = 1200;
	
	//send the length to the timer
	TCC0.CCB = length;
}

void pwmSetup()
{
	
	
//Math stuff
	
	//cpu frequency is 32MHz
	
	//we want a timer that overflows at 50Hz (period of 20mS)
	
	//32,000,000/50 = 640,000 cpu cycles in 20mS 
	
	//our timer resolution is 16-bit meaning the max it can count to is 2^16 -1.  That comes to 65535
	
	//we have timer divisors of 1,2,8,64,256, and 1024
	
	// 640,000/65535 = ~10 so we need a divisor of greater than 10.  Our best option is then divide by 64

	//640,000/64 = 10000 timer counts for a period of 20mS
	
	//By that math each count of the timer is 2uS
	
		
//Configs

	//we will be using PWM on pin 0 of the DAQ, which is controlled by the xmega's PORTC1 pin.
	//this pin has pwm functionality through timer TCC0 according to the atxmeg32a4u datasheet.
	
	//make the daq pin 0 an output
	PORTC.DIRSET = 0x02;
	
	//setup timer C0 to have a divisor of 64 as we decided upon in the math section 	
	TCC0.CTRLA = TC_CLKSEL_DIV64_gc;
	
	//config the timer's waveform generator for single slope operation.
	//This is a preferable mode for motor control.
	//If you wanna know the details of this setting consult the atxmega32A4u datasheet
	
	TCC0.CTRLB = TC_WGMODE_SINGLESLOPE_gc;
	
	//set the timer period to 10,000 as we calculated in the math section
	
	TCC0.PER = 10000;
	
	//Enable timer control (PWM) of our selected i/o pin
	TCC0.CTRLB = TC0_CCBEN_bm; 
	
}
